#include "encoder.h"

WheelEncoder::WheelEncoder(int input_a, int input_b):SimpleKalmanFilter(5,5,0.5), ESP32Encoder()
{   
    aPinNumber = (gpio_num_t)input_a;
    bPinNumber = (gpio_num_t)input_b;
    pinMode(input_a, INPUT_PULLDOWN);
    pinMode(input_b, INPUT_PULLDOWN);
    
}
WheelEncoder::WheelEncoder(int input_a, int input_b,bool change_dir):SimpleKalmanFilter(2,2,0.5), ESP32Encoder()
{   
    aPinNumber = (gpio_num_t)input_a;
    bPinNumber = (gpio_num_t)input_b;
    pinMode(input_a, INPUT_PULLDOWN);
    pinMode(input_b, INPUT_PULLDOWN);
    this->change_dir=change_dir;
}

void WheelEncoder::init_hardware(){
    useInternalWeakPullResistors=DOWN;
    attachHalfQuad(aPinNumber,bPinNumber);
    init_whell_param();
    setCount(0);
}

void WheelEncoder::init_whell_param(float wheel_d, float encoder_multiples, float encoder_precision, float reduction_ratio)
{
     _wheel_diameter = wheel_d;
     _encoder_multiples = encoder_multiples;
     _encoder_precision = encoder_precision;
     _reduction_ratio = reduction_ratio;
}
double  WheelEncoder::get_speed_raw(){
    long now_time=micros();
    long now_count=get_cnt();
    double dt=(now_time-last_speed_update_time)*0.000001f;
    // return encoderight_behind * Control_Frequency / EncoderMultiples / Reduction_Ratio / Encoder_precision * PI * Diameter_67;
    int delt_cnt=now_count-last_count;
    double speed=delt_cnt*PI/ _encoder_multiples/ _reduction_ratio/ _encoder_precision* _wheel_diameter/dt;
    last_speed_update_time=now_time;
    last_count=now_count;
    return speed;  
}

double  WheelEncoder::get_speed(){
    return updateEstimate(get_speed_raw());

}
float WheelEncoder::get_distance(){
    // location_left_front*PI*Diameter_67/EncoderMultiples/Reduction_Ratio/Encoder_precision;
    return get_cnt()*PI* _wheel_diameter/ _encoder_multiples/ _reduction_ratio/ _encoder_precision;
}
void WheelEncoder::encoder_reset(){
    setCount(0);
}
long WheelEncoder::get_cnt(){
    return this->change_dir?-getCount():getCount();
}
